#ifndef FTR3_DRIVER_HPP_
#define FTR3_DRIVER_HPP_

#include <rclcpp/rclcpp.hpp>
#include <can_msgs/msg/frame.hpp>
#include <std_msgs/msg/string.hpp>
#include "ftr3_driver/msg/ftr3_data.hpp"
#include "ftr3_driver/msg/ftr3_status.hpp"

#include <string>
#include <unordered_map>
#include <vector>
#include <cstdint>
#include <thread>
#include <mutex>
#include <atomic>

namespace ftr3_driver
{

struct Ftr3Config
{
    int32_t node_id = 31; // 默认 0x1F
    bool auto_start = false;
    bool invert_encoding = false;
    bool scaling_enabled = false;
    double angle_offset = 0.0;
};

class Ftr3Driver : public rclcpp::Node
{
public:
    Ftr3Driver();
    ~Ftr3Driver();

    bool init();
    void start();
    void stop();

private:
    // 配置参数
    Ftr3Config config_;
    
    // CAN通信相关
    rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr can_subscriber_;
    
    // 数据发布相关
    rclcpp::Publisher<ftr3_driver::msg::Ftr3Data>::SharedPtr data_publisher_;
    rclcpp::Publisher<ftr3_driver::msg::Ftr3Status>::SharedPtr status_publisher_;
    
    // 状态变量
    std::atomic<bool> is_running_;
    std::atomic<bool> is_initialized_;
    std::atomic<uint8_t> error_code_;
    std::atomic<double> current_angle_;
    std::atomic<int16_t> current_speed_;
    // 是否使用固定速率
    std::atomic<bool> use_fixed_rate_ = false;
    
    // 线程相关
    std::thread processing_thread_;
    std::mutex data_mutex_;
    
    // 配置文件加载
    void load_config();
    
    // CAN消息处理
    void can_callback(const can_msgs::msg::Frame::SharedPtr msg);
    
    // SDO消息处理
    void process_sdo_response(const can_msgs::msg::Frame::SharedPtr msg);
    
    // PDO消息处理
    void process_pdo(const can_msgs::msg::Frame::SharedPtr msg);
    
    // EMCY消息处理
    void process_emcy(const can_msgs::msg::Frame::SharedPtr msg);

    // 数据发布线程
    void processing_loop();
    
};

}  // namespace ftr3_driver

#endif  // FTR3_DRIVER_HPP_
    